/*
 * Robot.cpp
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#include "Robot.h"

namespace std {

Robot::Robot(Maze* maze, int width, int height, int picHeight, int picWidth) : speed(0.02),m(maze),width(width),height(height),showMap(true),showLikelyhoodMap(false){
	this->heading = 0;
	this->filter_init = false;
	this->hasFilter = false;
	this->resol = picWidth / width;
	chose_random_direction();
	this->p = maze->random_free_place();
	this->x = p.x;
	this->y = p.y;
	this->map = new Map(width,height,picHeight,picWidth);
}

void Robot::chose_random_direction(){
	this->heading = (UNIFORM_RAND)*360;
	cout << "new heading: "<< heading <<endl;
	if(hasFilter && filter_init) filter->setHeading(heading);
}

void Robot::enableFilter(bool hasFilter){
	this->hasFilter = hasFilter;
}

void Robot::initFilter(unsigned int n){
	this->filter = new LocalizationFilter(n,heading,resol,map);
	this->filter->create();
	this->filter_init = true;
	this->hasFilter = true;
}

void Robot::move(){
	int step_count = 0;
	while( true ){
		if(!advance_by(speed,m,false)){
			Point p (x+dx,y+dy);
			chose_random_direction();
			step_count++;
		}else{
			return;
		}
	}
}

void Robot::setMaps(bool map, bool lmMap,bool filterParts){
	this->showMap = map;
	this->showLikelyhoodMap = lmMap;
	this->showFilterParticles = filterParts;
}

void Robot::drawMap(){
	if(! drawn ){ map->draw(); drawn = true;}
	map->redraw();
	map->drawRobot(p,Point(dx,dy));
	if(hasFilter && showFilterParticles){
		for(unsigned int i = 0; i < filter->particles.size(); i++){
			map->drawParticles(filter->particles[i].p);
		}
	}
	map->repaint();
}

double Robot::getHeading(){
	return heading;
}

void Robot::predictPosition(double old_heading){
	filter->predict(old_heading,speed*resol);
}

double Robot::read_sensor(){
	int length = 5;
	int grad = 5;
	int startAngle = -90, endAngle = 90;
	double div = 10.0;
	vector <double> measure;
	vector <bool> walls;
	bool added;
	for(int i = startAngle; i <= endAngle; i+=grad){
		added = false;
		for(int radius = length; radius > 0; radius--){
			double xk = radius/div * cos(radians(heading+i));
			double yk = radius/div * sin(radians(heading+i));
			if(m->is_free(Point(p.x+xk,p.y+yk))){
				measure.push_back((radius/div));
				bool val = ((radius < length)?true:false);
				walls.push_back(val);
				added = true;
				break;
			}
		}
		if(added == false){
			measure.push_back(0);
			walls.push_back(true);
		}
	}
	Measurement mes(measure,walls,grad,(double)startAngle,(double)endAngle,heading,(length/div),(1/div),p);
	map->updateMap(mes);
	if(hasFilter){
		filter->measure(mes);
		filter->resample();
	}
	return add_little_noise(Particle::read_sensor(m));
}

Robot::~Robot() {

}

} /* namespace std */
